﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Globalization;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using Microsoft.Kinect;
using Microsoft.Kinect.Toolkit;
using Microsoft.Speech.AudioFormat;
using Microsoft.Speech.Recognition;

namespace KinectCamera
{
    /// <summary>
    /// MainWindow.xaml の相互作用ロジック
    /// </summary>
    public partial class MainWindow : Window
    {
        // Kinectセンサーオブジェクト。
        //KinectSensor kinect;
        // KinectSensorChooser
        private KinectSensorChooser kinectChooser = new KinectSensorChooser();

        // RGBカメラの解像度・フレームレート。
        ColorImageFormat rgbFormat = ColorImageFormat.RgbResolution640x480Fps30;
        // Kinectセンサーからの画像情報を受け取るバッファ。
        private byte[] pixelBuffer = null;
        // Kinectセンサから骨格情報を受け取るバッファ。
        private Skeleton[] skeletonBuffer = null;
        // 画面表示用ビットマップ。
        private RenderTargetBitmap bmpBuffer = null;
        // 顔マスクのビットマップイメージ。
        private BitmapImage maskImage = null;
        // 吹き出し用ビットマップイメージ。
        private BitmapImage fukidashiImage = null;
        // ビットマップへの描画用DrawingVisual。
        private DrawingVisual drawVisual = new DrawingVisual();

        // 音源の角度。
        private double soundDir = double.NaN;
        // 音声認識エンジン。
        private SpeechRecognitionEngine speechEngine;
        // 認識された文。
        private string recognizedText = null;

        public MainWindow()
        {
            InitializeComponent();
        }

        private void Window_Loaded(object sender, RoutedEventArgs e)
        {
            try
            {       
                // 画像の読み込み。
                Uri maskImgUri = new Uri("pack://application:,,,/images/gundam_san.jpg");
                maskImage = new BitmapImage(maskImgUri);
                Uri fukiImgUri = new Uri("pack://application:,,,/images/fukidashi.jpg");
                fukidashiImage = new BitmapImage(fukiImgUri);

                // KinectChoosserのイベントハンドラの登録。
                kinectChooser.KinectChanged += hKinectChanged;
                kinectChooser.PropertyChanged += hKinectPropChanged;
                
                kinectChooser.Start();
            }
            catch
            {
            }
        }

        // Kinectセンサーの初期化。
        private void initKinectSensor(KinectSensor kinect)
        {
            // カラーストリームの有効化。
            ColorImageStream clrStream = kinect.ColorStream;
            clrStream.Enable(rgbFormat);
            // 骨格ストリームの有効化。
            SkeletonStream skelStream = kinect.SkeletonStream;
            skelStream.Enable();                    

            // バッファの初期化。
            pixelBuffer = new byte[kinect.ColorStream.FramePixelDataLength];
            skeletonBuffer = new Skeleton[skelStream.FrameSkeletonArrayLength];
            bmpBuffer = new RenderTargetBitmap(clrStream.FrameWidth,
                                                    clrStream.FrameHeight,
                                                    96, 96, PixelFormats.Default);

            cameraImage.Source = bmpBuffer;
            
            // 音声認識エンジンの初期化。
            speechEngine = initSpeechEngine();

            // イベントハンドラの登録。
            kinect.AllFramesReady += hAllFramesReady;
            kinect.AudioSource.SoundSourceAngleChanged += hSoundSrcAngleChanged;
            speechEngine.SpeechRecognized += hSpeechRecognized;
            
            // kinectセンサからのストリームを取得。
            System.IO.Stream stream = kinect.AudioSource.Start();
            var speechAudioFormat
                = new SpeechAudioFormatInfo(EncodingFormat.Pcm,
                                            16000,
                                            16,
                                            1,
                                            32000,
                                            2,
                                            null);
            speechEngine.SetInputToAudioStream(stream, speechAudioFormat);
            speechEngine.RecognizeAsync(RecognizeMode.Multiple);
        }

        // Kinectセンサーの終了処理。
        private void uninitKinectSensor(KinectSensor kinect)
        {
            kinect.AllFramesReady -= hAllFramesReady;
            kinect.AudioSource.SoundSourceAngleChanged -= hSoundSrcAngleChanged;
            uninitSpeechEngine();
        }

        // ポーズ判定。
        private int checkPosture(Skeleton skeleton)
        {
            // 必要な関節・方向を取得。
            Joint head = skeleton.Joints[JointType.Head];
            Joint rwrist = skeleton.Joints[JointType.WristRight];
            Joint relbow = skeleton.Joints[JointType.ElbowRight];
            Joint lelbow = skeleton.Joints[JointType.ElbowLeft];
            Joint lshoulder = skeleton.Joints[JointType.ShoulderLeft];

            Matrix4 rwrDir = (skeleton.BoneOrientations[JointType.WristRight]
                                .AbsoluteRotation.Matrix);
            Matrix4 relbDir = (skeleton.BoneOrientations[JointType.ElbowRight]
                                .AbsoluteRotation.Matrix);

            // 一つでも位置が取れない場合は処理しない。
            if ((head.TrackingState != JointTrackingState.Tracked
                && head.TrackingState != JointTrackingState.Inferred)
                || (rwrist.TrackingState != JointTrackingState.Tracked
                    && rwrist.TrackingState != JointTrackingState.Inferred)
                || (relbow.TrackingState != JointTrackingState.Tracked
                    && relbow.TrackingState != JointTrackingState.Inferred)
                || (lelbow.TrackingState != JointTrackingState.Tracked
                    && lelbow.TrackingState != JointTrackingState.Inferred)
                || (lshoulder.TrackingState != JointTrackingState.Tracked
                    && lshoulder.TrackingState != JointTrackingState.Inferred))
                return 0;

            // チェック1.右手は頭より上にある。
            // = 右手首のY座標が頭のY座標より大きい。
            bool check1 = (rwrist.Position.Y > head.Position.Y);

            // チェック2.右手は斜め上に上げている。（真上ではない）
            // = 右手首の関節の向きのx成分またはz成分が0ではない。
            bool check2 = Math.Abs(rwrDir.M12) + Math.Abs(rwrDir.M32) > 0.3;

            // チェック3.右ひじがまっすぐ伸びている。
            // = 右ひじの関節の向きと右手首の関節の向きが同じ。(内積が1)
            bool check3 = (rwrDir.M12 * relbDir.M12
                            + rwrDir.M22 * relbDir.M22
                            + rwrDir.M32 * relbDir.M32) > 0.9;

            // チェック4.左手は下げている。
            // = 左ひじのY座標が左肩のY座標より小さい。
            bool check4 = (lelbow.Position.Y < lshoulder.Position.Y);

            return (check1 && check2 && check3 && check4) ? 1 : 0;
        }

        // Kinectセンサーの挿抜イベントに対し、初期化/終了処理を呼び出すハンドラ。
        private void hKinectChanged(object sender, KinectChangedEventArgs args)
        {
            if (args.OldSensor != null)
                uninitKinectSensor(args.OldSensor);

            if (args.NewSensor != null)
                initKinectSensor(args.NewSensor);
        }

        // Kinectセンサーのプロパティ変更時に呼ばれるハンドラ。
        private void hKinectPropChanged(object sender, PropertyChangedEventArgs args)
        {
            if ("Status".Equals(args.PropertyName))
            {
                textBlockStatus.Text = "Status:" + kinectChooser.Status;
            }
        }

        // SpeechRecognizedイベントのハンドラ。
        // 確度が一定以上の場合は認識された文を記録。
        private void hSpeechRecognized(object sender, SpeechRecognizedEventArgs e)
        {
            if (e.Result != null && e.Result.Confidence >= 0.3)
                recognizedText = e.Result.Text;
            else
                recognizedText = null;
        }

        // 音声認識エンジンの初期化。
        private SpeechRecognitionEngine initSpeechEngine()
        {
            RecognizerInfo targetRi = null;
            foreach (RecognizerInfo recognizer in SpeechRecognitionEngine.InstalledRecognizers())
            {
                if (recognizer.AdditionalInfo.ContainsKey("Kinect")
                    && "True".Equals(recognizer.AdditionalInfo["Kinect"],
                                    StringComparison.OrdinalIgnoreCase)
                    && "ja-JP".Equals(recognizer.Culture.Name,
                                    StringComparison.OrdinalIgnoreCase))
                {
                    targetRi = recognizer;
                    break;
                }
            }

            if (targetRi == null) return null;

            SpeechRecognitionEngine engine = new SpeechRecognitionEngine(targetRi.Id);

            // 認識する単語の追加。
            var words = new Choices();
            words.Add("キネクト");
            words.Add("テスト");
            words.Add("こんにちは");

            var grammarBuilder = new GrammarBuilder();
            grammarBuilder.Culture = targetRi.Culture;
            grammarBuilder.Append(words);
            var grammar = new Grammar(grammarBuilder);
            engine.LoadGrammar(grammar);

            return engine;
        }


        // SoundSouceAngleChangedイベントのハンドラ。
        // 確度（値の信頼度）が一定以上のときだけ角度を取得。
        private void hSoundSrcAngleChanged(object sender, SoundSourceAngleChangedEventArgs e)
        {
            soundDir = e.ConfidenceLevel > 0.3 ? e.Angle : double.NaN;
        }

        // FrameReadyイベントのハンドラ。
        // 画像情報を取得して、顔部分にマスク画像を上書きする。
        private void hAllFramesReady(object sender, AllFramesReadyEventArgs e)
        {
            KinectSensor kinect = sender as KinectSensor;
            List<Tuple<SkeletonPoint, Matrix4, int>> headList = null;

            // 骨格情報から、頭の座標リストを作成。
            using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
            {
                if (skeletonFrame != null)
                {
                    headList = getHeadPoints(skeletonFrame);
                }
            }

            // カメラの画像情報に、顔位置にマスク画像を描画する。
            using (ColorImageFrame imageFrame = e.OpenColorImageFrame())
            {
                if (imageFrame != null)
                {
                    fillBitmap(kinect, imageFrame, headList);
                }
            }
        }

        // 骨格情報から、頭の位置を取得してリストに入れる。
        private List<Tuple<SkeletonPoint, Matrix4, int>> getHeadPoints(SkeletonFrame skelFrame)
        {
            // 処理結果の空リスト作成。
            var results = new List<Tuple<SkeletonPoint, Matrix4, int>>();

            // 骨格情報をバッファにコピー。
            skelFrame.CopySkeletonDataTo(skeletonBuffer);

            // 取得できた骨格毎にループ。
            foreach (Skeleton skeleton in skeletonBuffer)
            {
                // トラッキングできていない骨格は処理対象外。
                if (skeleton.TrackingState != SkeletonTrackingState.Tracked)
                    continue;

                // ポーズをチェック。
                int pose = checkPosture(skeleton);

                // 骨格から頭を取得。
                Joint head = skeleton.Joints[JointType.Head];

                // 頭の位置が取得できていない場合は処理しない。
                if (head.TrackingState != JointTrackingState.Tracked
                    && head.TrackingState != JointTrackingState.Inferred)
                    continue;

                // 頭の向きを取得。
                Matrix4 headMtr = (skeleton.BoneOrientations[JointType.Head]
                                    .AbsoluteRotation.Matrix);

                // 頭の位置・向きを保存。
                results.Add(Tuple.Create(head.Position, headMtr, pose));
            }

            return results;
        }

        // RGBカメラの画像に顔マスクを上書きする。
        private void fillBitmap(KinectSensor kinect,
                                ColorImageFrame imgFrame,
                                List<Tuple<SkeletonPoint, Matrix4, int>> headList)
        {
            // 描画準備。
            var drawContext = drawVisual.RenderOpen();
            int frmWidth = imgFrame.Width;
            int frmHeight = imgFrame.Height;

            // 画像情報をバッファにコピー。
            imgFrame.CopyPixelDataTo(pixelBuffer);

            // カメラの画像情報から背景ビットマップを作成。
            var bgImg = new WriteableBitmap(frmWidth, frmHeight, 96, 96,
                                            PixelFormats.Bgr32, null);
            bgImg.WritePixels(new Int32Rect(0, 0, frmWidth, frmHeight),
                            pixelBuffer, frmWidth * 4, 0);
            drawContext.DrawImage(bgImg, new Rect(0, 0, frmWidth, frmHeight));

            // getHeadPointsで取得した各頭部の位置毎にループ。
            for (int idx = 0; headList != null && idx < headList.Count; ++idx)
            {
                // 骨格の座標から画像情報の座標に変換。
                SkeletonPoint headPos = headList[idx].Item1;
                ColorImagePoint headPt
                    = kinect.MapSkeletonPointToColor(headPos, rgbFormat);

                // 距離に応じてサイズを決定。
                int size = (int)(192 / headPos.Z);
                // ポーズをとると顔のサイズを2倍に。
                if (headList[idx].Item3 == 1) size *= 2;

                // 頭の位置に頭の向きに回転させたマスク画像を表示。
                Matrix4 headMtrx  = headList[idx].Item2;
                Matrix rot = new Matrix(headMtrx.M11, -headMtrx.M12,
                                        -headMtrx.M21, headMtrx.M22,
                                        headPt.X, headPt.Y);
                drawContext.PushTransform(new MatrixTransform(rot));
                Rect rect = new Rect(headPt.X - size/2, headPt.Y - size/2, size, size);
                drawContext.DrawImage(maskImage, rect);
                drawContext.Pop();

                // 頭の近くに音源を検出した場合は吹き出しを描画。
                double angle = Math.Atan2(headPos.X, headPos.Z) * 180 / Math.PI;
                if (Math.Abs(soundDir - angle) < 10)
                {
                    Rect frect = new Rect(headPt.X - 200, headPt.Y - 100, 150, 80);
                    drawContext.DrawImage(fukidashiImage, frect);

                    // 音声を認識している場合は吹き出しにテキストを表示。
                    if (recognizedText != null)
                    {
                        var text = new FormattedText(recognizedText,
                                                    CultureInfo.GetCultureInfo("ja-JP"),
                                                    FlowDirection.LeftToRight,
                                                    new Typeface("Verdana"),
                                                    24,
                                                    Brushes.Black);
                        var pt = new Point(headPt.X - 200 + 12, headPt.Y - 100 + 12);
                        drawContext.DrawText(text, pt);
                    }
                }
            }

            // 画面に表示するビットマップに描画。
            drawContext.Close();
            bmpBuffer.Render(drawVisual);
        }

        // Windowクローズのハンドラ。
        private void Window_Closed(object sender, EventArgs e)
        {
            kinectChooser.Stop();
        }

        // 音声認識エンジンの終了処理。
        private void uninitSpeechEngine()
        {
            if (speechEngine != null)
            {
                speechEngine.SpeechRecognized -= hSpeechRecognized;
                speechEngine.Dispose();
                speechEngine = null;
            }
        }

    }
}
